#include "MotorController.hpp"
#include "Configuration.hpp"
#include "system.h"
#include "adc.hpp"
#include "pwm_input.hpp"
#include "eeprom_device.hpp"
#include "BLE.hpp"
#include <stdio.h>
#include "string.h"
#include "indicator.hpp"
#include "fault_info.hpp"
/* 库文件 */
#include "apm32a10x_rcm.h"
#include "apm32a10x_tmr.h"
#include "apm32a10x_misc.h"
#include "apm32a10x_wwdt.h"
#include "apm32a10x_iwdt.h"
#include "apm32a10x_gpio.h"
#include "ff.h"

#include "CommProtocol.hpp"
#include "Protocol_impl.hpp"
#include "updater.h"
#include "crc16.hpp"

#include "key.hpp"
#include "key_event.hpp"


class _Configurator_Portable : public Configurator {
public:
    virtual int read(void *buff,unsigned int address,int size){
        return size;
    }
    virtual int write(void *buff,unsigned int address,int size){
        return size;
    }
};

class App {
public:
    void init();
    void eventLoop();

};

BLE *ble;
MotorController *motor_controller;
_Configurator_Portable *configurator_portable;
ADCUtil *adc_util;
PWMInput *pwm_input;
M24C64 *eeprom;
LED *led;
Beeper *beeper;
KEY *btkey;
KEYEvent *keyevent;
FaultInfo *faultinfo;

//定时时间 分配任务优先级
unsigned int SystemTick = 0; 

//SD卡更新完成标志位 
static bool sd_updater_flag = false; 

/* 电机初始标志位 */
bool motormin_init_finished = true;   //电机最短极限位置初始化完成标志
bool motormax_init_finished = true;   //电机最长极限位置初始化完成标志

// 初始化完成标志 主要判断电机极限位置校准是否完成
bool init_finished = true; 

/* 声明默认和实时配置变量  */
extern SystemConfiguration default_system_configuration;
extern MotionCharacteristicData default_motion_characteristic_data;

/* 存储器加载的时候 已经加载了默认数据 */
SystemConfiguration system_configuration ={};
MotionCharacteristicData system_motion_characteristic_data = {};


void App::init(){

    printf("System init_v 17:21\r\n");

    int ret = 0;
    sd_updater_flag = false;
    ret = Updater_checkSDCard_v2();
    sd_updater_flag = true;
    /* 实例化 */
    adc_util = new ADCUtil();
    pwm_input = new PWMInput();
    configurator_portable = new _Configurator_Portable();
    motor_controller = new MotorController(adc_util);
    ble = new BLE();
    keyevent = new KEYEvent();
    faultinfo = new FaultInfo();


    /*  初始化 */
    adc_util->init();
    pwm_input->init();
    motor_controller->init();
    led = led->getInstance();

    btkey = btkey->getInstance();
    led->set(true);

    // motor_controller->setPosition(0.5);

    /* System timer config. Use TMR6. */
    System_setPeripheralClockEnable(CLK_TMR6,TRUE);
    TMR_Reset(TMR6);
    TMR_BaseConfig_T system_timer_base_configration;
    system_timer_base_configration.division = 71;
    system_timer_base_configration.clockDivision = TMR_CLOCK_DIV_1;
    system_timer_base_configration.period = 99;
    system_timer_base_configration.repetitionCounter = 0;
    system_timer_base_configration.countMode = TMR_COUNTER_MODE_UP;
    TMR_ConfigTimeBase(TMR6,&system_timer_base_configration);
    TMR_EnableInterrupt(TMR6,TMR_INT_UPDATE);
    NVIC_EnableIRQRequest(TMR6_IRQn,0,1);
    TMR_Enable(TMR6);

    /* 独立看门狗 */
    if(RCM_ReadStatusFlag(RCM_FLAG_IWDTRST) == SET){
        RCM_ClearStatusFlag();
    }
    IWDT_EnableWriteAccess();
    IWDT_ConfigDivider(IWDT_DIVIDER_32);
    IWDT_ConfigReload(300);
    IWDT_Enable();

    // init_sync(); // 清空eeprom配置

    /* 获取唯一的96位序列号 在此处将default_system_configuration的芯片ID赋值*/
    default_system_configuration.chip_id[0] = *(__IO uint32_t *)(0X1FFFF7F0); // 高字节
	default_system_configuration.chip_id[1] = *(__IO uint32_t *)(0X1FFFF7EC); // 
	default_system_configuration.chip_id[2] = *(__IO uint32_t *)(0X1FFFF7E8); // 低字节
    /* 读取外部存储 判断是否加载默认配置项 */
    init_load();
    motor_controller->updatePli();
    // 修改固件版本 该信息无法被用户修改
    system_configuration.firmware_version[0] = 0x01;
    system_configuration.firmware_version[1] = 0x00;
    system_configuration.firmware_version[2] = 0x00;
    system_configuration.firmware_version[3] = 0x09;
    for( int i = 0; i < 4; i++){
        printf("%d ",system_configuration.firmware_version[i]);
    }
    printf("\r\n");
    // 通过 XOR 操作将 96 位的 ID 压缩为一个 32 位的标识符
    uint32_t unique_id = default_system_configuration.chip_id[0] ^ default_system_configuration.chip_id[1] ^ default_system_configuration.chip_id[2];
    char unique_id_str[9];  // 8 个字符 + 1 个 NULL 终止符
    char ble_name[20];  // 20 字符长度的缓冲区
    sprintf(unique_id_str, "%08X", unique_id);
    snprintf(ble_name, sizeof(ble_name), "CarBLE-%s", unique_id_str);
    printf("ID %s\r\n",ble_name);
    printf("Load finished \r\n");
    printf("%08x\r\n",default_system_configuration.chip_id[0]);
    printf("%08x\r\n",default_system_configuration.chip_id[1]);
    printf("%08x\r\n",default_system_configuration.chip_id[2]);
    /* SD卡升级 */


    beeper = beeper->getInstance();
    beeper->set(false);
    /* 设置蓝牙名 */
    ble->setName(ble_name);

    CommProtocolPortable_t portable_impl ={
        .authenticate = authenticate_impl,
        .randKey = randKey_impl,
        .sendResponse = sendResponse_impl,
        .readData = readData_impl,
        .writeData = writeData_impl,
        .startOTA = startOTA_impl,
        .writeOTAData = writeOTAData_impl,
        .verifyOTA = verifyOTA_impl,
    };
    CommProtocol_init(portable_impl);

    /* 上电电机位置校准开关 */
    if( system_configuration.calibration_switch == 1){
        motormin_init_finished = false;
        motormax_init_finished = false;
        init_finished = false;
    }else{
        motormin_init_finished = true;
        motormax_init_finished = true;
        init_finished = true;
    }

    /* 初始化已经读取所有存储值 判断电机极限位置是否在正常范围内*/
    if( system_configuration.motor_minlength_voltage < 1.5 || system_configuration.motor_maxlength_voltage > 1.5){
        system_configuration.motor_minlength_voltage = default_system_configuration.motor_minlength_voltage;
        system_configuration.motor_maxlength_voltage = default_system_configuration.motor_maxlength_voltage;
        /* 电机校准 */
        motormin_init_finished = false;
        motormax_init_finished = false;
        init_finished = false;
        printf("read voltage error\r\n");

    }
    printf("min %f\r\n",system_configuration.motor_minlength_voltage);
    printf("max %f\r\n",system_configuration.motor_maxlength_voltage);
    

    /* 判断蓝牙开关状态 */
    if( system_configuration.bluetooth_switch == 1){
        ble->open();
        led->set(TRUE);
    }else{
        ble->close();
        led->set(FALSE);
    }
    SystemTick = 0;
    
    /* 初始化 重新加载一下线性插值数据 */
    motor_controller->updatePli();
    motor_controller->setPosition(0.5);

}


/**任务 */
char task_flag[10]={0};

/* 初始化电机极限位置相关变量*/
static double current_adc = 0;
static double last_adc = 0;
static double min_adc = 0;
static double max_adc = 0;
static unsigned int stable_count = 0;
static const int stable_threshold = 100; 

/* 电机控制过程变量 */
static double duty;
static double actual_duty;
static double current_position;

void App::eventLoop(){

    int ret = 0;

    /* 电机极限位置初始化 以及阈值标定 */
    if ( task_flag[6] == 1  ) {
        if(motormin_init_finished == false && motormax_init_finished == false){
            motor_controller->setForce(0.6);
            current_adc = adc_util->getChannel(ADCUtil::Channel::Hall);
            if( (last_adc - current_adc ) < 0.05 ){
                stable_count ++;
            }else {
                stable_count = 0;
            }
            if (stable_count >= stable_threshold) {
                max_adc = current_adc;
                last_adc = 0;
                stable_count = 0;
                motor_controller->setForce(0); 
                motormax_init_finished = true;    
                
               // printf("Motor max position initialized with ADC value: %f\r\n", max_adc);
                EEPROM *eeprom = getEEPROM();
                uint32_t address = 0x00000000;
                uint32_t offset = 0x00000000;
                offset = (unsigned int)(&system_configuration.motor_maxlength_voltage) - (unsigned int)(&system_configuration);
                eeprom->write(&max_adc,address+offset,sizeof(system_configuration.motor_maxlength_voltage));
                eeprom->read(&system_configuration.motor_maxlength_voltage,address+offset,sizeof(system_configuration.motor_maxlength_voltage));
                printf("motor_maxlength_voltage Read: %f\r\n", system_configuration.motor_maxlength_voltage);
                // 读取的数据不合适 
                if( system_configuration.motor_maxlength_voltage > 1.5) {
                    printf("motor_maxlength_voltage read error\r\n");
                    system_configuration.motor_maxlength_voltage = default_system_configuration.motor_maxlength_voltage;
                }
            }

        }
        
        else if( motormin_init_finished == false && motormax_init_finished == true){

            motor_controller->setForce(-0.6);
            current_adc = adc_util->getChannel(ADCUtil::Channel::Hall);

            if( (current_adc - last_adc) < 0.05 ){
                stable_count ++;
            }else {
                stable_count = 0;
            }

            if (stable_count >= stable_threshold) {

                last_adc = 3.3;
                stable_count = 0;
                motor_controller->setForce(0); 
                System_delayMs(500);
                current_adc = adc_util->getChannel(ADCUtil::Channel::Hall);
                min_adc = current_adc;

                motormin_init_finished = true;    
                
                EEPROM *eeprom = getEEPROM();
                uint32_t address = 0x00000000;
                uint32_t offset = 0x00000000;
                offset = (unsigned int)(&system_configuration.motor_minlength_voltage) - (unsigned int)(&system_configuration);
                eeprom->write(&min_adc,address+offset,sizeof(system_configuration.motor_minlength_voltage));
                eeprom->read(&system_configuration.motor_minlength_voltage,address+offset,sizeof(system_configuration.motor_minlength_voltage));
                printf("motor_minlength_voltage Read: %f\r\n", system_configuration.motor_minlength_voltage);
                // 读取数据不合适
                if( system_configuration.motor_minlength_voltage < 1.5) {
                    printf("motor_minlength_voltage read error\r\n");
                    system_configuration.motor_minlength_voltage = default_system_configuration.motor_minlength_voltage;

                }
                /* 电机极限位置矫正 */
                init_finished = true;
                SystemTick = 0;
            }
        }
        task_flag[6] = 0;
    }

    /* 电机控制环 */
    if ( task_flag[0] == 1  ) {
        if(motormin_init_finished == true && motormax_init_finished == true){
            /* 在函数内部进行处理电机位置反转 仅读取*/
            motor_controller->loop();
            current_position = motor_controller->getPosition();
        } 
        task_flag[0] = 0;
    }


    /* 电机位置获取 */
    if ( task_flag[1] == 1 ){
        if( motormin_init_finished == true && motormax_init_finished == true){
            duty = pwm_input->getDuty();
            if( duty < 0.007 ) duty = 0.0005;
            if( duty > 0.994)   duty = 0.9995;
            if ( duty < system_configuration.motor_position_minimum ) duty = system_configuration.motor_position_minimum;
            if ( duty > system_configuration.motor_position_maximum ) duty = system_configuration.motor_position_maximum;
            
            if(duty < 0) duty = 0;
            if(duty > 1) duty = 1;

            actual_duty  = duty;
            /* 判断输入duty开关是否开启 决定duty的值是如何计算*/
            if( system_configuration.inputduty_switch == 1){
                actual_duty = 1 - duty;
            }
            /* 在函数内部 判断电机位置计算开关 实际运行*/
            motor_controller->setPosition(actual_duty);
            task_flag[1]=0;
        }
    }

    /* 蓝牙接收任务 */
    if ( task_flag[2] == 1){
        if(ble->getStatus() == true) ble->eventLoop();
        task_flag[2] = 0;
    }

    /* 心跳包任务以及打印相关信息 */
    if ( task_flag[3] == 1 ){
        CommProtocol_loop();
        // printf("error %d\r\n",faultinfo->get_error());
        // printf("force %.2f\r\n",motor_controller->getForce());
        /* 打印电机相关数据 */
        // printf("n_data_points %d\r\n",system_motion_characteristic_data.n_data_points);
        // for( int i = 0; i < system_motion_characteristic_data.n_data_points; i++){
        //     printf("%f ",system_motion_characteristic_data.x_data[i]);
        // }
        // printf("\r\n");
        // for( int i = 0; i < system_motion_characteristic_data.n_data_points; i++){
        //     printf("%f ",system_motion_characteristic_data.y_data[i]);
        // }
        // printf("\r\n");

        // if( motormin_init_finished == true && motormax_init_finished == true){
        //     motor_controller->debugPrint();
        // }


        // printf("duty %f\r\n",duty);
        // printf("actual_duty %f\r\n",actual_duty);
        // printf("\r\n");

        task_flag[3] = 0;
    }

    /* 位置同步 已弃用*/
    if ( task_flag[4] == 1 ){
        //data_sync(motor_position,&system_configuration.motor_position);
        task_flag[4] = 0;
    }

    /* 按键任务*/
    if ( task_flag[5] == 1){

        ret = keyevent->keyEvent(SystemTick / 10);
        if( ret == one_clicked) printf("one_clicked\r\n");
        else if( ret == double_clicked){
            printf("double_clicked\r\n");
            if( faultinfo->get_error() == motor_error) faultinfo->set_error(none_error);
        } 
        else if( ret == triple_clicked) printf("triple_clicked\r\n");
        else if( ret == long_pressed_3s) {

            if ( ble->getStatus() == true){
                /* 同步蓝牙状态 */
                system_configuration.bluetooth_switch = 0;
                data_sync(bluetooth_switch,&system_configuration.bluetooth_switch);
                ble->close();
                led->set(false);
                beeper->beeper_time(1);
            }else{
                /* 同步蓝牙状态 */
                system_configuration.bluetooth_switch = 1;
                data_sync(bluetooth_switch,&system_configuration.bluetooth_switch);
                ble->open();
                led->set(true);
                beeper->beeper_count(2);
            }
        }

        task_flag[5] = 0;
    }

    /* 错误事件 */
    if( task_flag[7] = 1){
        faultinfo->error_event();
        task_flag[7] = 0;
    }
}

extern "C"
int main(){
    printf("System boot\r\n");
    App app;
    app.init();
    while ( true ){
        app.eventLoop();
    }
}

/* 错误事件触发次数 */
static int error_count = 0;
static int full_force_count = 0;
const int max_error_count = 1;

/* 0.1 ms */
extern "C" __attribute__((interrupt()))
void TMR6_IRQHandler(void){
    if ( TMR_ReadIntFlag(TMR6,TMR_INT_UPDATE) != RESET ){
        TMR_ClearIntFlag(TMR6,TMR_INT_UPDATE);

        if ( SystemTick++ >= 1000000 ) SystemTick = 0;

        IWDT_Refresh();

        /* 电机控制环 */
        if(SystemTick % 10 == 0) task_flag[0]=1;
        
        /* 获取电机位置以及初始化 */
        if(SystemTick % 100 == 0) task_flag[1] = 1;   
    
        //蓝牙接收任务   
        if(SystemTick % 100 == 0) task_flag[2] = 1;   
        
        /* 心跳任务 */
        if(SystemTick % 10000 == 0) task_flag[3]=1;

        /* SD卡更新状态 */
        if(SystemTick % 10000 == 0){
            if( sd_updater_flag == false){
                led->set(!led->get());
            }else{
                if(ble->getStatus() == true) led->set(true);
                else led->set(false);
            }
        }

        /* 电机位置同步 */
        if(SystemTick % 100000 == 0) task_flag[4]=1;
        
        /* bt key*/
        if(SystemTick % 1000 == 0) task_flag[5]=1;
              
        /* 电机极限位置初始化 */
        if(SystemTick % 100 == 0) task_flag[6] = 1;  

        /* 故障事件处理 */
        if(SystemTick % 100 == 0) task_flag[7] = 1; 

        /* 电机初始化ADC同步*/
        if(SystemTick % 3000 == 0){
            if(init_finished == false){
                last_adc = current_adc;
            }

        }
        
        if( sd_updater_flag == true){
        /* 电机故障检测*/
        if(SystemTick % 50000 == 0){

            if( init_finished == true){
                /* 满驱报错 未接电机可模拟 */
                if(motor_controller->getForce() > 2 ||  motor_controller->getForce() < -2){
                    full_force_count++;
                    if( full_force_count > max_error_count){
                            init_finished = false;  //  只报警一次
                            printf("set error_1\r\n");
                            faultinfo->set_error(motor_error);
                            full_force_count = 0;
                    }
                }else{
                    full_force_count = 0;
                }
                
                /* 电机静止时 检错 */
                if( motor_controller->getForce() < 0.1 &&  motor_controller->getForce() > -0.1){
                    if( (motor_controller->getPositionSetPoint() - current_position) > 0.1 || (motor_controller->getPositionSetPoint() - current_position) < -0.1){
                        error_count++;
                        if (error_count > max_error_count) {
                            init_finished = false;  //  只报警一次
                            printf("set error_2\r\n");
                            faultinfo->set_error(motor_error);
                            error_count = 0;
                        }
                    } else {
                        // 如果没有误差，可以重置计数器
                        error_count = 0;
                    }
                }else{
                    error_count = 0;
                }

            }

        }   
        }
     
    }
    return;
}
